'''
    IMU驱动配置文件
    author: Draco
    2022/12/21
'''
import utime
order_list = {
    # 所有数据输出
    'all_output':[0xA5,0x55,0x53,0x4D],
    # 加速度数据输出
    'acc_output':[0xA5,0x55,0x01,0xFB],
    # 100Hz输出
    '100HZ' : [0xA5,0x59,0x03,0x01],
    # 水平姿态
    'flat' :[0xA5,0x5B,0x02,0x02],
    # 保存设置
    'save_order': [0xA5,0x5A,0x01,0x00],
    # 进行水平校准
    'correct_order' : [0xA5,0x57,0x01,0xFD]
    }
def order_send(order,uart):
    for i in order:
        uart.write(i.to_bytes(1, 'little', False))
    utime.sleep_ms(50)
    
def judge_frame(uart):
    snt = 0
    while(snt!=2):
        frame_flag = uart.read(1)
        if frame_flag == b'Z':
            snt += 1
        else:
            snt = 0
    return True

def bytesSolving(bytes_data):
    ans = ((bytes_data[0]<<8 )| bytes_data[1])
    
#     ans = int(bin(ans)[3:],2)
    if ans & 0x8000 == 0x8000:
        ans = (~ans+1)&0xffff
        ans = -1 *ans
    return ans

# 23bytes data process
def dataSolving(data):
    dataset = {}
    # 加速度计
    dataset['ACC_X'] = bytesSolving(data[2:4])/16383.5
    dataset['ACC_Y'] = bytesSolving(data[4:6])/16383.5
    dataset['ACC_Z'] = bytesSolving(data[6:8])/16383.5
#     # 陀螺仪
#     dataset['GYRO_X'] = bytesSolving(data[8:10])/16.3835
#     dataset['GYRO_Y'] = bytesSolving(data[10:12])/16.3835
#     dataset['GYRO_Z'] = bytesSolving(data[12:14])/16.3835
#     # 欧拉角
#     dataset['ROLL'] = bytesSolving(data[14:16])/100
#     dataset['PITCH'] = bytesSolving(data[16:18])/100
#     dataset['YAW'] = bytesSolving(data[18:20])/100
#     # 温度
#     dataset['TEMP'] = bytesSolving(data[20:22])/100
    
    return dataset


if __name__ == "__main__":
    a = b'\x3f\x2d$\xe2\xf6\x12\x18\xff\xff\xff\xfe\x00\x00\x16\x7f\x16e\xe2\xf9\x08z\xde'
    b = dataSolving(a)
    print(b)
    print(order_list)